/*
 * timer0.c
 *
 * Created: 6.11.2013 18:20:22
 *  Author: Ferda
 */ 

#include "timer0.h"
#include "util.h"
#include <avr/interrupt.h>

//=======================================================================================

#ifdef USE_TIMER0

static uint32_t vect_millis __attribute__((section("bss")));
static uint32_t aux_millis __attribute__((section("bss")));
static uint32_t change_flag __attribute__((section("bss"))); 

ISR(TIMER0_COMPA_vect, ISR_NAKED)
{
	asm volatile (
		"push	r24					\n\t"
		"in		r24, __SREG__		\n\t"
		"push	r24					\n\t"
		"push	r25					\n\t"
		
		"lds	r24,%[vect]			\n\t"
		"lds	r25, %[vect]+1		\n\t"
		"adiw	r24, 1				\n\t"
		"sts	%[vect], r24		\n\t"
		"sts	%[vect]+1, r24		\n\t"
		"brcc	end%=				\n\t"
		
		"lds	r24, %[vect]+2		\n\t"
		"lds	r25, %[vect]+3		\n\t"
		"adiw	r24, 1				\n\t"
		"sts	%[vect]+2,r24		\n\t"
		"sts	%[vect]+3,r25		\n\t"
		
		"end%=:						\n\t"
		"ser	r24					\n\t"
		"sts	%[flag], r24		\n\t"
		
		"pop	r25					\n\t"
		"pop	r24					\n\t"
		"out	__SREG__, r24		\n\t"
		"pop	r24					\n\t"
		"reti						\n\t"
		
		:
		: [vect] "i" (&vect_millis), [flag] "i" (&change_flag)
		:
	);
}

//=======================================================================================

OPTIMIZE_SIZE void TIMER0_init()
{
	vect_millis = aux_millis = 0;
	change_flag = 0;
	
	TIFR0 = 0;
	TCNT0 = 0;
	OCR0A = 250;							// counter max value
	TCCR0A = (1 << WGM01);					// CTC mode
	TCCR0B = (1 << CS00) | (1 << CS01);		// Prescaler 1/64
}

//=======================================================================================

__attribute__((naked, noreturn)) uint32_t TIMER0_millis()
{
	asm volatile (
		"lds	r24, %[flag]				\n\t"
		"tst	r24							\n\t"
		"breq	ret_old%=					\n\t"
		
		"cli								\n\t"
		"lds	r22, %[vect]				\n\t"
		"lds	r23, %[vect]+1				\n\t"
		"lds	r24, %[vect]+2				\n\t"
		"lds	r25, %[vect]+3				\n\t"
		"sts	%[flag], __zero_reg__		\n\t"
		"sei								\n\t"
		
		"sts	%[aux],r22					\n\t"
		"sts	%[aux]+1, r23				\n\t"
		"sts	%[aux]+2, r24				\n\t"
		"sts	%[aux]+3, r25				\n\t"
		"ret								\n\t"
		
		"ret_old%=:							\n\t"
		"lds	r22, %[aux]					\n\t"
		"lds	r23, %[aux]+1				\n\t"
		"lds	r24, %[aux]+2				\n\t"
		"lds	r25, %[aux]+3				\n\t"
		"ret								\n\t"
	:
	: [vect] "i" (&vect_millis), [flag] "i" (&change_flag), [aux] "i" (&aux_millis)
	:
	);
}

//=======================================================================================


#endif